#include"stick.h"
using namespace std;

double surface1(ParFor1 par, Point P) {
	return P.x*P.x*par.a + P.y*P.y*par.b + P.z*P.z*par.c + par.d;
}

double surface2(ParFor2 par, Point P, Point S) {
	return (P.x-S.x)*(P.x-S.x)/par.a + (P.y-S.y)*(P.y-S.y)/par.b + \
			  (P.z-S.z)*(P.z-S.z)/par.c + par.d;
}

double tangential1(ParFor1 par1, ParFor2 par2, Point P, Point S) {
	return par1.a*P.x/(par1.c*P.z) - par2.a*(P.x-S.x)/(par2.c*(P.z-S.z));
}

double tangential2(ParFor1 par1, ParFor2 par2, Point P, Point S) {
	return par1.b*P.y/(par1.c*P.z) - par2.b*(P.y-S.y)/(par2.c*(P.z-S.z));
}

double dPhi(Point S, ParForPhi par) {
	double phiS = atan(S.y/S.x);
	if(S.x == 0) {
		if(S.y > 0) phiS = M_PI/2;
		else if(S.y == 0) return 0;
		else phiS = 3*M_PI/2;
	}
	else if(S.x < 0) {
		phiS += M_PI;
	}
	else if(S.y < 0) {
		phiS += 2*M_PI;
	}
	double phiCurrent = par.current*2*M_PI/par.precision;
	return phiS - phiCurrent;
}

int function(const gsl_vector *x, void *params, gsl_vector *y) {
	Parameters *par = (Parameters*)params;
	Point P;
		P.x = gsl_vector_get (x, 0);
		P.y = gsl_vector_get (x, 1);
		P.z = gsl_vector_get (x, 2);
	Point S;
		S.x = gsl_vector_get (x, 3);
		S.y = gsl_vector_get (x, 4);
		S.z = gsl_vector_get (x, 5);
	
	fprintf(stderr, "%f,%f,%f: ", P.x, P.y, P.z);
	fprintf(stderr, "%f, ", surface1(par->for1, P));
	fprintf(stderr, "%f\n", surface2(par->for2, P, S));

	gsl_vector_set (y, 0, surface1(par->for1, P));
	gsl_vector_set (y, 1, surface2(par->for2, P, S));
	gsl_vector_set (y, 2, tangential1(par->for1, par->for2, P, S));
	gsl_vector_set (y, 3, tangential2(par->for1, par->for2, P, S));
	gsl_vector_set (y, 4, S.z-par->zPlain);
	gsl_vector_set (y, 5, dPhi(S, par->forPhi));

	return GSL_SUCCESS;
}

void print_state (size_t iter, gsl_multiroot_fsolver * s) {
	fprintf (stderr, "iter = %3u, P = ", iter);
	for(int i=0; i<3; i++)
		{ fprintf(stderr, "%.3e, ", gsl_vector_get(s->x,i)); }  
	fprintf(stderr, "S = ");
	for(int i=3; i<6; i++)
		{ fprintf(stderr, "%.3e, ", gsl_vector_get(s->x,i)); }  
	fprintf(stderr, "\n\tf = ");
	for(int i=0; i<6; i++)
		{ fprintf(stderr, "%.3e, ", gsl_vector_get(s->f,i)); }  
	fprintf(stderr, "\n");
}

void useGSL(int(*fun)(const gsl_vector *x, void *par, gsl_vector *y),\
		void *par, int n, double *x0, int current, double **results) {

	int status;
	size_t iter = 0;

	gsl_multiroot_function f = {fun, n, par};

	gsl_vector *x = gsl_vector_alloc (n);
	for(int i=0; i<n; i++) { gsl_vector_set (x, i, x0[i]); }
	
	const gsl_multiroot_fsolver_type *T;
	T = gsl_multiroot_fsolver_hybrids;
	gsl_multiroot_fsolver *s;
	s = gsl_multiroot_fsolver_alloc (T, n);
	gsl_multiroot_fsolver_set (s, &f, x);

	print_state (iter, s);

	do
	{
		iter++;
		status = gsl_multiroot_fsolver_iterate (s);
		print_state(iter, s);
		if(status) break; //check if solver is stuck
		status = gsl_multiroot_test_residual (s->f, 1e-15);
	}
	while (status == GSL_CONTINUE && iter < 1000);

	// print result
	printf("%d: status = %s, iter = %d: ", \
		((Parameters*)par)->forPhi.current, gsl_strerror (status), iter);
	double x3 = gsl_vector_get(s->x,3);
	double x4 = gsl_vector_get(s->x,4);
	printf(" %.6e\t%.6e\n", x3, x4);

	// get result
	for(int i=0; i<n; i++)
		{ results[i][current] = gsl_vector_get(s->x,i); }

	gsl_multiroot_fsolver_free (s);
	gsl_vector_free (x);
}


// use Haron's formula 
double calculateArea(double* x, double *y, int N) {
	double area = 0;

	for(int i=1; i<N; i++) {
		double a = sqrt(x[i]*x[i]+y[i]*y[i]);
		double b = sqrt(x[i-1]*x[i-1]+y[i-1]*y[i-1]);
		double c = sqrt((x[i-1]-x[i])*(x[i-1]-x[i]) +\
			(y[i-1]-y[i])*(y[i-1]-y[i]));
		double p = (a+b+c)/2;
		area += sqrt(p*(p-a)*(p-b)*(p-c));
	}
	return area;
}
